import cv2
import numpy as np
import sift
import math
import key_frame
import patch_track
from MDFlow_main.demo_md import flowtrack,line_track

def angle_between_vectors(vector1, vector2):#vector1由匹配点和中心点组成的向量，vector1由平行的矩形角点和中心点组成的向量
    norm_vector1 = np.linalg.norm(vector1) # 模长
    norm_vector2 = np.linalg.norm(vector2)
    dot_product = np.dot(vector1, vector2)  # 计算向量的点积
    cos_theta = dot_product / (norm_vector1 * norm_vector2)#cos值
    theta = np.arccos(np.clip(cos_theta, -1.0, 1.0)) # 使用反余弦函数计算角度（弧度）
    angle_degrees = np.degrees(theta)# 将弧度转换为度（范围0-180，0-30内算可接受范围内）
    # cross_product = np.cross(vector1, vector2)  # 叉乘,判断从1到2的方向
    # if cross_product > 0:
    #     angle_degrees=angle_degrees#逆时针
    # elif cross_product < 0:
    #     angle_degrees = -angle_degrees#顺时针
    # else:
    #     angle_degrees=0
    return angle_degrees


def point_rot(angle,point,center):#点绕点旋转
    # sign = math.sign(angle)
    valuex = np.array(point[0])
    valuey = np.array(point[1])
    pointx=np.array(center[0])
    pointy = np.array(center[1])
    if angle>0:# 绕pointx,pointy逆时针旋转
        Rotatex = (valuex - pointx) * math.cos(angle) - (valuey - pointy) * math.sin(angle) + pointx
        Rotatey = (valuex - pointx) * math.sin(angle) + (valuey - pointy) * math.cos(angle) + pointy
    if angle<0:#顺时针
        Rotatex = (valuex-pointx)*math.cos(angle) + (valuey-pointy)*math.sin(angle) + pointx
        Rotatey =-(valuex-pointx)*math.sin(angle) + (valuey-pointy)*math.cos(angle) + pointy
    rot_point=np.array([Rotatex,Rotatey])
    return rot_point



def M_aff(resized_img1, resized_img2):#仿射变换
    kp1, des1 = sift.getkp(resized_img1)
    kp2, des2 = sift.getkp(resized_img2)
    good1, good2 = sift.sift_points(kp1, des1, kp2, des2)
    d2 = np.float32(good2[0:3, :])
    d1 = np.float32(good1[0:3, :])
    M = cv2.getAffineTransform(d2, d1)
    img2_rot = cv2.warpAffine(img2, M, (img2.shape[1], img2.shape[0]))
    return img2_rot


def creat_mask_(img,box,angle):#获取矩形区域的像素值
    center=(int(box[4][0]),int(box[4][1]))
    # rotation_matrix = cv2.getRotationMatrix2D(center, angle_degrees, scale=1)  #旋转中心，旋转角度，缩放
    # rotated_rect = cv2.transform(box[:4], rotation_matrix)  #对矩形的四个角点应用旋转矩阵,需要排序
    rot_rect = np.empty((4, 2))
    for i in range(4):
        point=box[i]
        rot_rect[i]=point_rot(angle,point,center)
    rot_rect_copy=rot_rect.copy()
    rot_rect[2], rot_rect[3] = rot_rect_copy[3], rot_rect_copy[2]
    rot_rect=rot_rect.astype(int)
    mask=np.zeros_like(img)
    img_=cv2.fillConvexPoly(mask,rot_rect,(255,255,255))#旋转区域
    rot_pt = np.argwhere(np.all(img_ == [255, 255, 255], axis=-1))#返回元素索引
    patch =img[rot_pt[:, 0], rot_pt[:, 1]]#旋转区域rgb

    # cv2.namedWindow("0", cv2.WINDOW_NORMAL)
    # cv2.imshow("Filled Polygon", mask)
    # cv2.waitKey(0)
    rotation_matrix_ = cv2.getRotationMatrix2D(center, -angle, scale=1)#将旋转后的框再恢复回去
    rotated_rect_ = cv2.transform(rot_pt, rotation_matrix_)
    uv_rgb=np.concatenate((rotated_rect_ ,patch),axis=1)
    # 使用numpy的lexsort函数对矩阵进行排序
    sorted_indices = np.lexsort(( uv_rgb[:, 0],uv_rgb[:, 1]))#先y后x
    # 使用排序后的索引重新排列矩阵
    sorted_matrix = uv_rgb[sorted_indices]
    reshaped_matrix = np.reshape(uv_rgb[:,2:5], (-1, box[5][0]))#得到旋转后的rgb
    return reshaped_matrix


def H_box_rot(H,img2,center_p,l):#img2_box:center_p
    center=center_p.T
    img3 = cv2.warpPerspective(img2, H, (img1.shape[1], img1.shape[0]))    # warp image B onto image A
    center_p3 = np.dot(H, np.vstack((center + 0.5, np.ones((1, center.shape[1])))))  # 进行像素中心对齐,像素坐标被认为是在像素的角落。添加0.5将它们移动到像素中心。
    center_p3 = center_p3 [0:2, :] / center_p3 [2, :] - 0.5
    box_3=patch_track.boxin(center_p3[0], center_p3[1], l, img3)

    # img3_mask = img3[box_3[0, 1]:box_3[2, 1] + 1, box_3[0, 0]:box_3[1, 0] + 1, :]
    # black_num = np.sum(np.all(img3_mask == (0, 0, 0), axis=2))#black_num
    # black_per=(black_num)/(l*l)
    # if black_per>0.3:

    return box_3


# def no_blackbox():


if __name__ == '__main__':
    import os
    # path="/big_data/zjy/Hamlyn/rectified06/image01/"#尺度变化
    # path = "/big_data/zjy/Hamlyn/rectified08/image01/"#器械翻组织
    # path = "/big_data/zjy/Hamlyn/rectified16/image01/"
    # dirs_init1 = os.listdir(path)
    # dirs_init1.sort(key=lambda x: int(x[:-4]))
    # for i in range(len(dirs_init1)):
    #     img1_rgb = cv2.imread(path + '/{}'.format(dirs_init1[i]), flags=1)
    #
    #     cv2.namedWindow("0", cv2.WINDOW_NORMAL)
    #     cv2.imshow("0", img1_rgb)
    #     cv2.waitKey(1)

    img1=cv2.imread("/home/ltt/point_track/rot_img/hamlyn_left_rotate/00000.png",flags=1)
    img2= cv2.imread("/home/ltt/point_track/rot_img/hamlyn_left_rotate/00095.png", flags=1)

    kp1, des1 = sift.getkp(img1)
    kp2, des2 = sift.getkp(img2)
    good1, good2 = sift.sift_points(kp1, des1, kp2, des2)
    point1=good1.T#good n*2,point 2*n
    point2 = good2.T
    H_init, _ = cv2.findHomography(good2, good1, method=cv2.RANSAC, ransacReprojThreshold=16 * np.sqrt(2) + 1, maxIters=5000,
                                  confidence=0.9999)
    # opencv might return None for H, check for None
    H_init = np.eye(3, dtype=np.double) if H_init is None else H_init
    # warp image B onto image A
    img3 = cv2.warpPerspective(img2, H_init, (img1.shape[1], img1.shape[0]))

    u_f, v_f, _, _ = flowtrack(img1, img2)
    goodf2, _, _ = line_track(u_f, v_f, good1, img2)
    img1_line = key_frame.line_trackpoint(img1, good1)
    img2_line = key_frame.line_trackpoint(img2, goodf2)
    img=np.concatenate((img1_line, img2_line), axis=0)
    cv2.imwrite(os.path.join('/home/ltt/point_track/rot_img/result6_10', 'flow_NOwrap.jpg'), img)




    #使用光流
    img1_mask=img1.copy()
    mask = np.all(img3 < 1, axis=-1)  # w x h, 得到黑色区域的布尔掩码
    # 将 a 中对应的区域设置为黑色
    img1_mask[mask] = 0
    u_f, v_f, _, _=flowtrack(img1_mask,img3)

    good3, _, _=line_track(u_f, v_f, good1, img3)
    point3=good3.T
    point3_2 = np.dot(np.linalg.inv(H_init), np.vstack((point3, np.ones((1, point3.shape[1])))))
    point3_2 = point3_2[0:2, :] / point3_2[2, :]
    good3_2=point3_2.T
    img3_line = key_frame.line_trackpoint(img3, good3)
    img1_line = key_frame.line_trackpoint(img1_mask, good1)
    img2_line = key_frame.line_trackpoint(img2, good2)

    img3_2line = key_frame.line_trackpoint(img2, good3_2)
    img = np.concatenate((img1_line, img2_line), axis=1)
    img_ = np.concatenate((img3_line, img3_2line), axis=1)
    img__ = np.concatenate((img, img_), axis=0)

    cv2.imwrite(os.path.join('/home/ltt/point_track/rot_img/result6_10', 'flow_NOmask.jpg'), img__)

    cv2.namedWindow("0", cv2.WINDOW_NORMAL)
    cv2.imshow("Filled Polygon", img__)
    cv2.waitKey(0)

    # # project points B to warped image C
    # point3 = np.dot(H_init, np.vstack((point2 + 0.5, np.ones((1, point2.shape[1])))))#进行像素中心对齐,像素坐标被认为是在像素的角落。添加0.5将它们移动到像素中心。
    # point3= point3[0:2, :] / point3[2, :] - 0.5  #point3 2 x n
    # # boxcenter_3 = np.dot(H_init, np.vstack((boxcenter_2.T + 0.5, np.ones((1, 1)))))
    # good3=point3.T
    # point3_2 = np.dot(np.linalg.inv(H_init), np.vstack((point3, np.ones((1, point3.shape[1])))))
    # point3_2 = point3_2[0:2, :] / point3_2[2, :]
    # good3_2 = point3_2.T
    #
    # img1_line=key_frame.line_trackpoint(img1, good1)
    # img2_line=key_frame.line_trackpoint(img2, good2)
    # img3_line = key_frame.line_trackpoint(img3, good3)
    # img3_2line=key_frame.line_trackpoint(img2, good3_2)
    # img=np.concatenate((img1_line,img2_line),axis=1)
    # img_ = np.concatenate((img3_line, img3_2line), axis=1)
    # img__ = np.concatenate((img, img_), axis=0)
    #
    # cv2.imwrite(os.path.join('/home/ltt/point_track/rot_img/result6_10', 'wrap.jpg'), img__)
    #
    # cv2.namedWindow("0", cv2.WINDOW_NORMAL)
    # cv2.imshow("Filled Polygon",img__)
    # cv2.waitKey(0)
